/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.ManualDrive;

/**
 *
 * @author Jarrod Risley
 */
public class DrivetrainSubsystem extends PIDSubsystem {

    /*
     * Global Class Variables
     */
    private Victor foreVictor;
    private Victor rearVictor;
    private Encoder encoder;
    private static Relay leds;
    
    private static final double Kp = 7.0;
    private static final double Ki = 0.5;
    private static final double Kd = 2.0;

    private static final double MIN_IMPULSE_DURATION = 0.5;
    private static final double TIME_TO_TRAVERSE_UNIT_OF_DISTANCE = 1; // CALIBRATE!!!!
    
    private String subsystemName;
    private double lastCommandTime = 0;
    private boolean motorsEnabled;
    private double impulseDuration;

    
    // CONSTRUCTOR //
    
    
    /**
     * Creates one instance of a drivetrain. Note that the robot will use two 
     * of these for left 
     * 
     * @param name The name of the subsystem.
     * @param sidecarNum Which sidecar the Victors are attached to.
     * @param pwmPort The port for both Victors.
     * @param encoderChannelA The Subsystem's Encoder's Channel A.
     * @param encoderChannelB The Subsystem's Encoder's Channel B.
     */
    public DrivetrainSubsystem(String name, int sidecarNum, int forePort, 
            int rearPort, int encoderChannelA, int encoderChannelB, boolean isInverted) {
        
        super("DrivetrainSubsystem", Kp, Ki, Kd);

        System.out.println("Creating DrivetrainSubsystem " + name + "...");
        subsystemName = name;
        
        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
        
        foreVictor = new Victor(sidecarNum, forePort);
        rearVictor = new Victor(sidecarNum, rearPort);
        
        encoder = new Encoder(encoderChannelA, encoderChannelB, isInverted);
            encoder.start();
            
//        LiveWindow.addActuator(subsystemName, "foreVictor", foreVictor);
//        LiveWindow.addActuator(subsystemName, "rearVictor", rearVictor);
//        LiveWindow.addSensor(subsystemName, "PID Controller", getPIDController());
//        LiveWindow.addSensor(subsystemName, "Encoder", encoder);
            
    } // end Drivetrain Subsystem Constructor 
    
    
    // METHODS //
    
    
    /**
     * For this subsystem, this is where the thumbsticks will be mapped.
     */
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        
        setDefaultCommand(new ManualDrive());
        
    } // end initDefaultCommand
    
    /**
     * Returns the sensor value for the PID controller (that is invisible here).
     * @return The encoder reading in terms of (units).
     */
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        return Math.abs(encoder.getRaw());
    } // end returnPIDInput
    
    /**
     * The computed output value is applied to the motors...DON'T USE THIS 
     * METHOD - it works on its own since PIDSubsystem's PIDController uses 
     * this.
     * 
     * @param output - Not to be tampered with.
     */
    
    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        
        foreVictor.set(-output * .75);
        rearVictor.set(-output * .75);
        System.out.println(subsystemName + " encoder reads: " + encoder.getRaw());
        
        if ((motorsEnabled) && (Timer.getFPGATimestamp() > lastCommandTime 
                + impulseDuration)) {
            
            motorsEnabled = false;
            disable();
            
        } // end if
        
    } // end usePIDOutput
    
    /**
     * This method is only for use when driving via operator input. If you're
     * calling this method where the robot is acting autonomously - STOP! 
     * Rethink your strategy. 
     * 
     * @param thumbstickValue - The value of the thumbstick.
     */
    
    public void operatorControl(double thumbstickValue) {
        
        foreVictor.set(thumbstickValue);
        rearVictor.set(thumbstickValue);
        
        System.out.println(subsystemName + " encoder reads: " + encoder.getRaw());
        
    } // end operatorControl
    
    /**
     * Commands the robot to move a set distance in (unit of measurement). Note
     * that this method should only be used in autonomous operations - NOT 
     * OPERATOR CONTROL!
     *  
     * @param distance - The distance in (units) for the robot to travel.
     * @return True if the robot has reached the set destination.
     */
    
    public void commandDistance(double distance) {
        
       System.out.println(subsystemName + " is moving to " + distance 
               + " (units)");
       enable();
       impulseDuration = MIN_IMPULSE_DURATION + TIME_TO_TRAVERSE_UNIT_OF_DISTANCE * (distance - getLocation());
       motorsEnabled = true;
       lastCommandTime = Timer.getFPGATimestamp();
       setSetpoint(distance);
        
    } // end commandDistance
    
    /**
     * Gets the current output of the encoder.
     * 
     * @return returnPIDInput()
     */
    
    public double getLocation() {
        return returnPIDInput();
    } // end getLocation
    
    /**
     * Checks to see if an autonomous command is running currently.
     * 
     * @return True if another autonomous command is running.
     */
    
    public boolean areMotorsEnabled() {
        return motorsEnabled;
    } // end areMotorsEnabled
    
    /**
     * Resents the encoder to zero.
     */
    
    public void resetEncoder() {
        encoder.reset();
    } // emd resetEncoder
    
    public static void activateLEDS() {
        leds.set(Relay.Value.kOn);
    } // end activateLEDS
    
    public static void deactivateLEDS() {
        leds.set(Relay.Value.kOff);
    } // end activateLEDS
    
} // end DrivetrainSubsystem
